#ifndef __FOC_H
#define __FOC_H


#include "my_main.h"

#define MOTOR_ENABLE HAL_GPIO_WritePin(DRV_EN_GPIO_Port,DRV_EN_Pin,GPIO_PIN_SET)
#define MOTOR_DISENABLE HAL_GPIO_WritePin(DRV_EN_GPIO_Port,DRV_EN_Pin,GPIO_PIN_RESET)
#define PWM_Period 1599

#define _2_SQRT3 1.15470053838
#define _SQRT3 1.73205080757
#define _1_SQRT3 0.57735026919
#define _SQRT3_2 0.86602540378
#define _SQRT2 1.41421356237
#define _120_D2R 2.09439510239
#define _PI 3.14159265359
#define _PI_2 1.57079632679
#define _PI_3 1.0471975512
#define _2PI 6.28318530718
#define _3PI_2 4.71238898038
#define _PI_6 0.52359877559


float FirstOrderLPF(float input, float prev_output, float alpha);
float SqrtApprox(float number);
float NormalizeAngle(float angle);
float ElectricalAngle(float shaft_angle, int pole_pairs);



typedef struct{

	float * _Uab;
	float * _Udq;
	
	void(*Init)(void);
	void(*RevPark)(float* Uin, float* Uout, float angle);
	void(*SvpwmGenerate)(float* Uin);
	void(*SetUdq)(float ud, float uq);
}Foc;

extern Foc foc;
#endif
